import copy
import math
import threading
import time
import os

import airsim
from dataclasses import dataclass
from airsim import Pose, Vector3r
import pandas as pd
from typing import List, Tuple
import pymap3d as pm
import numpy as np

# ORCA半平面
class Plane:
    def __init__(self, point: np.ndarray = np.zeros(3), normal: np.ndarray = np.zeros(3)):
        # 面上的一点
        self.point: np.ndarray = point
        # 面的法向量
        self.normal: np.ndarray = normal


class UavFly(threading.Thread):
    def __init__(self, number, goals, settings,fly_on_hand,radius_all):
        super(UavFly, self).__init__()
        self.client = airsim.MultirotorClient()
        self.client.enableApiControl(True, vehicle_name=number)
        self.client.armDisarm(True, vehicle_name=number)
        self.client.takeoffAsync(vehicle_name=number).join()
        # 当前无人机编号
        self.number = number
        self.goals = np.array(goals)
        self.settings = settings
        self.radius_all = radius_all
        self.inv_time_horizon = 1 / settings[number].time_horizon
        self.duration = settings[number].time_horizon/2
        self.is_running = True
        self.fly_on_hand = fly_on_hand
    # 结束当前线程
    def stop(self):
        self.is_running = False

    # 获取无人机经纬度和速度
    def get_multirotor_state(self, vehicle_name):
        state = self.client.getMultirotorState(vehicle_name=vehicle_name)
        latitude = state.gps_location.latitude
        longitude = state.gps_location.longitude
        altitude = state.gps_location.altitude
        x_val = state.kinematics_estimated.linear_velocity.x_val
        y_val = state.kinematics_estimated.linear_velocity.y_val
        z_val = state.kinematics_estimated.linear_velocity.z_val
        return latitude, longitude, altitude, x_val, y_val, z_val

    def run(self):
        # 如果是有人机则直接进行飞行 模仿有人机的飞行路线
        if self.number==self.fly_on_hand:
            self.client.moveToPositionAsync(self.goals[0][0],self.goals[0][1],self.goals[0][2],-1).join()
            self.client.moveToPositionAsync(self.goals[1][0], self.goals[1][1], self.goals[1][2], -1).join()
        # 如果是无人机
        # while self.goals[1][2]<self.client.simGetGroundTruthKinematics(self.number).position.z_val:
        #     self.client.moveByVelocityAsync(0,0,-1,self.duration,vehicle_name=self.number)
        self.client.moveToPositionAsync(self.goals[0][0], self.goals[0][1], self.goals[0][2], -1).join()
        while self.is_running:
            # 获得无人机的位置和速度信息
            state = self.client.getMultirotorState(vehicle_name=self.number)
            # 当前无人机的位置在airsim坐标系下
            current_val=np.array([state.kinematics_estimated.position.x_val,state.kinematics_estimated.position.y_val,state.kinematics_estimated.position.z_val])
            # 无人机当前经纬度
            latitude = state.gps_location.latitude
            longitude = state.gps_location.longitude
            altitude = state.gps_location.altitude
            # 无人机当前速度
            this_velocity = np.array([state.kinematics_estimated.linear_velocity.x_val,state.kinematics_estimated.linear_velocity.y_val, state.kinematics_estimated.linear_velocity.z_val])
            # 无人机距离目标点的距离，用来计算pref_velocity
            relative_position_goal = self.goals[1]-current_val
            # 无人机行驶时最佳速度
            pref_velocity = relative_position_goal / np.linalg.norm(relative_position_goal) * self.settings[self.number].max_speed
            pref_velocity_list = pref_velocity.tolist()
            # 获取当前区域内无人机的所有无人机名称
            vehicle_names = self.client.listVehicles()
            # 获取除当前无人机以外的其他无人机信息
            vehicle_names.remove(self.number)
            orca_planes = []
            for vehicle_name in vehicle_names:
                latitude_other, longitude_other, altitude_other, x_val_other, y_val_other, z_val_other = self.get_multirotor_state(
                    vehicle_name=vehicle_name)
                velocity = np.array([x_val_other, y_val_other, z_val_other])
                # 根据经纬度计算和当前无人机的相对距离
                e, n, u = pm.geodetic2ned(latitude_other, longitude_other, altitude_other, latitude, longitude,
                                          altitude)
                relative_position = np.array([e, n, u])
                dist = np.linalg.norm(relative_position)
                # 当相对距离小于两架无人机结合起来的冲突预测距离
                if dist <= np.linalg.norm(this_velocity) * self.settings[self.number].time_horizon + np.linalg.norm(velocity) *self.settings[
                vehicle_name].time_horizon:
                    plane = Plane()
                    relative_velocity = this_velocity - velocity
                    # 两架无人机的碰撞半径，将两架无人机视为质点
                    combined_radius = self.radius_all[self.settings[self.number].model] + self.radius_all[self.settings[vehicle_name].model]
                    dist_sq = dist ** 2
                    combined_radius_sq = combined_radius ** 2
                    a = dist_sq
                    b = relative_position.dot(relative_velocity)
                    c = relative_velocity.dot(relative_velocity) - np.linalg.norm(
                        np.cross(relative_position, relative_velocity)) ** 2 / (
                                dist_sq - combined_radius_sq)
                    t = (b + math.sqrt(b ** 2 - a * c)) / a
                    ww = relative_velocity - t * relative_position
                    ww_length = np.linalg.norm(ww)
                    plane.normal = ww / ww_length
                    u = (combined_radius * t - ww_length) * plane.normal
                    if vehicle_name == self.fly_on_hand:
                        # 针对有人机选择自己避让
                        plane.point = this_velocity + u
                    # 如果无人机距离在第一个碰撞层
                    elif dist <= np.linalg.norm(this_velocity) * self.settings[self.number].time_horizon + np.linalg.norm(velocity) *self.settings[
                vehicle_name].time_horizon:
                        # # 针对无人机默认双方避让
                        plane.point = this_velocity + 0.25 * u
                    # 如果无人机在第二个碰撞层
                    else:
                        plane.point = this_velocity + 0.5 * u
                    orca_planes.append(plane)
            if orca_planes:
                new_velocity, plane_length = self.linear_program3(orca_planes, self.settings[self.number].max_speed,
                                                                  pref_velocity, False, this_velocity)
                new_velocity = new_velocity.tolist()
                self.client.moveByVelocityAsync(new_velocity[0], new_velocity[1], new_velocity[2],
                                                vehicle_name=self.number, duration=self.duration)
            else:
                self.client.moveByVelocityAsync(pref_velocity_list[0], pref_velocity_list[1],
                                                pref_velocity_list[2], vehicle_name=self.number,
                                                duration=self.duration)
            time.sleep(self.settings[self.number].time_horizon)

    def linear_program1(self, planes: List[Plane], plane_no: int, line: Tuple[np.ndarray, np.ndarray], radius: float,
                        opt_velocity: np.ndarray, direction_opt: bool, result: np.ndarray) -> bool:

        dot_product = line[0].dot(line[1])

        discriminant = dot_product ** 2 + radius ** 2 - np.linalg.norm(line[0]) ** 2
        if discriminant < 0.0:
            return False

        sqrt_discriminant = np.sqrt(discriminant)
        t_left = -dot_product - sqrt_discriminant
        t_right = -dot_product + sqrt_discriminant
        for i in range(plane_no):
            numerator = (planes[i].point - line[0]).dot(planes[i].normal)
            denominator = line[1].dot(planes[i].normal)
            if abs(denominator) <= 1e-6:
                if numerator > 0.0:
                    return False
                else:
                    continue

            t = numerator / denominator
            if denominator >= 0.0:
                t_left = max(t_left, t)
            else:
                t_right = min(t_right, t)

            if t_left > t_right:
                return False
        if direction_opt:
            if opt_velocity.dot(line[1]) > 0.0:
                result[:] = line[0] + t_right * line[1]
            else:
                result[:] = line[0] + t_left * line[1]
        else:
            t = line[1].dot(opt_velocity - line[0])
            if t < t_left:
                result[:] = line[0] + t_left * line[1]
            elif t > t_right:
                result[:] = line[0] + t_right * line[1]
            else:
                result[:] = line[0] + t * line[1]

        return True

    def linear_program2(self, planes: List[Plane], plane_no: int, radius: float, opt_velocity: np.ndarray,
                        direction_opt: bool, result: np.ndarray) -> bool:
        plane_dist = planes[plane_no].point.dot(planes[plane_no].normal)
        plane_dist_sq = plane_dist ** 2
        radius_sq = radius ** 2
        # 判断最大速度是否大于平面的直线距离
        if plane_dist_sq > radius_sq:
            return False
        # 计算平面圆的半径
        plane_radius_sq = radius_sq - plane_dist_sq
        # 计算平面圆的圆心
        plane_center = plane_dist * planes[plane_no].normal
        if direction_opt:
            plane_opt_velocity = opt_velocity - (opt_velocity.dot(planes[plane_no].normal)) * planes[plane_no].normal
            plane_opt_velocity_sq = math.pow(np.linalg.norm(plane_opt_velocity), 2)
            if plane_opt_velocity_sq <= 1e-6:
                result[:] = plane_center
            else:
                result[:] = plane_center + np.sqrt(plane_radius_sq / plane_opt_velocity_sq) * plane_opt_velocity
        else:
            result[:] = opt_velocity + ((planes[plane_no].point - opt_velocity).dot(planes[plane_no].normal)) * planes[
                plane_no].normal
            if np.linalg.norm(result) > radius:
                plane_velocity = opt_velocity - opt_velocity.dot(planes[plane_no].normal) * planes[plane_no].normal
                plane_velocity_norm = plane_velocity / np.linalg.norm(plane_velocity)
                result[:] = plane_center + math.sqrt(plane_radius_sq) * plane_velocity_norm
        for i in range(plane_no):
            # 判断当前result是否在当前plane的范围内 >0代表不在范围内，需要再判断
            if planes[i].normal.dot(planes[i].point - result[i]) > 0:
                cross_product = np.cross(planes[i].normal, planes[plane_no].normal)
                if (cross_product[0] * cross_product[0] + cross_product[1] * cross_product[1] + cross_product[2] *
                    cross_product[2]) <= 1e-6:
                    return False
                line_normal = np.cross(cross_product, planes[plane_no].normal)
                line_point = planes[plane_no].point + (
                        ((planes[i].point - planes[plane_no].point).dot(planes[i].normal)) / (
                    line_normal.dot(planes[i].normal))) * line_normal
                if not self.linear_program1(planes, i, (line_point, line_normal), radius, opt_velocity, direction_opt,
                                            result):
                    return False
        return True

    def linear_program3(self, planes: List[Plane], radius: float, opt_velocity: np.ndarray, direction_opt: bool,
                        new_velocity: np.ndarray) -> int:
        if direction_opt:
            new_velocity[:] = opt_velocity * radius
        elif (opt_velocity[0] * opt_velocity[0] + opt_velocity[1] * opt_velocity[1] + opt_velocity[2] * opt_velocity[
            2]) > radius * radius:
            new_velocity[:] = opt_velocity / np.linalg.norm(opt_velocity) * radius
        else:
            new_velocity[:] = opt_velocity
        for i, pl in enumerate(planes):
            if pl.normal.dot(pl.point - new_velocity) > 0.0:
                temp_result = copy.deepcopy(new_velocity)

                if not self.linear_program2(planes, i, radius, opt_velocity, direction_opt, new_velocity):
                    new_velocity[:] = temp_result
                    return new_velocity, i

        return new_velocity, len(planes)


# 用于出现相关无人机
def generate_uav(goals):
    client = airsim.MultirotorClient()
    for key, goal in goals.items():
        if key != "000":
            pose = Pose(Vector3r(goal[0][0], goal[0][1], 0))
            client.simAddVehicle(key, "simpleflight", pose)
            print("无人机" + key + "创建成功。")



#用来记录调参时无人机数据
class report_velocity_and_relative_position(threading.Thread):
    def __init__(self, folder_path,goals, settings,radius_all):
        super().__init__()
        # 控制airsim无人机的对象
        self.client = airsim.MultirotorClient()
        # 用来控制线程的结束
        self.is_running = True
        # 记录无人机飞行数据的文件夹地址
        self.folder_path = folder_path
        # 无人机的目标地址
        self.goals=goals
        self.settings = settings
        self.radius_all = radius_all

    def stop(self):
        self.is_running = False

    def run(self):
        # “000”和“001”无人机相加的碰撞半径，将两架无人机都视为一个质点
        radius=self.radius_all[self.settings["000"].model]+self.radius_all[self.settings["001"].model]
        # 记录无人机飞行时的数据
        data = {}
        # 记录无人机位置的
        data["000"]=[]
        data["001"]=[]
        # 记录无人机速度的
        data["000_velocity"] = []
        data["001_velocity"] = []
        # 记录无人机碰撞半径的 当成质点相对的
        data["radius"]=[]
        # 根据速度计算的冲突距离
        data["neighbor_dist"]=[]
        # 无人机的相对位置
        data["distance"] = []
        while self.is_running:
            # 获取“000”无人机的数据
            state0 = self.client.simGetGroundTruthKinematics("000")
            # 获取"000"无人机在airsim坐标系下的位置
            state0_val = np.array([state0.position.x_val, state0.position.y_val, -state0.position.z_val])
            # 无人机当前速度
            state0_vel=np.array([state0.linear_velocity.x_val, state0.linear_velocity.y_val, -state0.linear_velocity.z_val])
            state1 = self.client.simGetGroundTruthKinematics("001")
            state1_val = np.array(
                [state1.position.x_val, state1.position.y_val, -state1.position.z_val]) + np.array(
                [self.goals["001"][0][0], self.goals["001"][0][1], 0])
            state1_vel=np.array([-state1.linear_velocity.x_val, -state1.linear_velocity.y_val, -state1.linear_velocity.z_val])
            # “000”和“001”无人机的相对位置
            distance = np.linalg.norm(state1_val - state0_val)
            # 根据无人机当前速度计算冲突距离
            neighbor_dist =np.linalg.norm(state0_vel) * self.settings["000"].time_horizon + np.linalg.norm(state1_vel) *self.settings[
                "001"].time_horizon
            if distance > neighbor_dist:
                continue
            data["000"].append(state0_val.tolist())
            data["001"].append(state1_val.tolist())
            data["000_velocity"].append(state0_vel.tolist())
            data["001_velocity"].append(state1_vel.tolist())
            data["radius"].append(radius)
            data["neighbor_dist"].append(neighbor_dist)
            data["distance"].append(distance)
        relative_position = pd.DataFrame(data)
        file_name="datasource.csv"
        relative_position.to_csv(f'{self.folder_path}/{file_name}',index=False)
        print("datasource.csv.csv数据成功")

class reach_goals(threading.Thread):
    def __init__(self, goals, uav_flys, report_velocity_and_relative_position):
        super().__init__()
        self.goals = goals
        self.goal = np.array(goals["000"][1])
        self.client = airsim.MultirotorClient()
        # self.report_position = report_position
        # self.report_velocity = report_velocity
        self.uav_flys = uav_flys
        self.report_velocity_and_relative_position = report_velocity_and_relative_position

    def run(self):
        state = self.client.simGetGroundTruthKinematics(vehicle_name="000")
        x_val = state.position.x_val
        y_val = state.position.y_val
        z_val = state.position.z_val
        while (np.linalg.norm(self.goal - np.array([x_val, y_val, z_val]))) > 10:
            state = self.client.simGetGroundTruthKinematics(vehicle_name="000")
            x_val = state.position.x_val
            y_val = state.position.y_val
            z_val = state.position.z_val
        # self.report_position.stop()
        # self.report_velocity.stop()
        self.report_velocity_and_relative_position.stop()
        for key in self.goals.keys():
            self.uav_flys[key].stop()





class PlotRouteLine(threading.Thread):
    def __init__(self, number, goal):
        super(PlotRouteLine, self).__init__()
        self.dt = 0.01
        self.client = airsim.MultirotorClient()
        self.is_running = True
        self.number = number
        self.intial_position = np.array([[goal[0][0]], [goal[0][1]], [0]])

    def run(self):
        UAV_state = self.client.simGetGroundTruthKinematics(vehicle_name=self.number)
        pos_start = np.array(
            [[UAV_state.position.x_val], [UAV_state.position.y_val], [UAV_state.position.z_val]]) + self.intial_position
        plot_last_pos = [airsim.Vector3r(pos_start[0, 0], pos_start[1, 0], pos_start[2, 0])]
        while self.is_running:
            UAV_state = self.client.simGetGroundTruthKinematics(vehicle_name=self.number)
            pos_now = np.array([[UAV_state.position.x_val], [UAV_state.position.y_val],
                                [UAV_state.position.z_val]]) + self.intial_position
            vel_now = np.array([[UAV_state.linear_velocity.x_val], [UAV_state.linear_velocity.y_val],
                                [UAV_state.linear_velocity.z_val]])
            plot_v_start = [airsim.Vector3r(pos_now[0, 0], pos_now[1, 0], pos_now[2, 0])]
            plot_v_end = pos_now + vel_now
            plot_v_end = [airsim.Vector3r(plot_v_end[0, 0], plot_v_end[1, 0], plot_v_end[2, 0])]
            self.client.simPlotArrows(plot_v_start, plot_v_end, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0])
            self.client.simPlotLineList(plot_last_pos + plot_v_start, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness=8,
                                        is_persistent=True)
            plot_last_pos = plot_v_start
            time.sleep(self.dt)

    def stop(self):
        self.is_running = False

@dataclass
class Settings:
    # 无人机最大速度
    max_speed: float
    # 无人机当前型号
    model:str
    # 无人机预测冲突的时间
    time_horizon: float

# 两架无人机
if __name__ == '__main__':
    # 飞行模式：若model=1则代表碰撞距离判断只有一个圈，若model=2则代表碰撞距离的判断有两个圈
    model=1
    # 存放无人机飞行过程数据的文件夹
    folder_path="UAV-HAV-5-2-25-5--3"+"model-"+str(model)
    if not os.path.exists(folder_path):
        os.makedirs(folder_path)
    # 有人机编号
    fly_on_hand = ""
    # 提前预测冲突的时间
    prediction_time=2
    # 碰撞半径，根据不同无人机型号确定碰撞半径，“0”:3代表型号为0的无人机的碰撞半径为3，实际上无人机实际的机身半径小于这个值
    radius_all={"0":3,"1":6,"2":9}
    # 这里为设置每架无人机的设置
    settings_all = {"000": Settings(6, "0",  prediction_time), "001": Settings(6, "0", prediction_time)}
    # 为无人机的位置设置，这个距离设置为在airsim的坐标系下的设置
    goals = {"000": [[0, 0, -10], [200, 200, -10]], "001": [[199, 201, -10], [-200, -200, -10]]}
    # 因为airsim启动时只有一架无人机，所以对于多出来的无人机需要进行创建
    generate_uav(goals)
    # report_position = report_position(folder_path, goals,settings_all)
    # report_position.start()
    # 用来记录每架无人机飞行控制线程
    uav_flys = {}
    # 用来记录在airsim中绘制无人机轨迹的线程
    plot_route_lines = {}
    # 用来记录无人机飞行过程中的数据
    report_velocity_and_relative_position=report_velocity_and_relative_position(folder_path, goals,settings_all,radius_all)
    report_velocity_and_relative_position.start()
    for key, goals in goals.items():
            uav_flys[key] = UavFly(key, goals, settings_all, fly_on_hand,radius_all)
            uav_flys[key].start()
            plot_route_lines[key] = PlotRouteLine(key, goals)
            plot_route_lines[key].start()
    # 判断“000”无人机是否到达目的地的线程

    reach_goals = reach_goals(goals,  uav_flys, report_velocity_and_relative_position)
    reach_goals.start()
#


